Inner Loop Controller

Now that we have made a working outer loop controller that is successfully able to fly the crazyflie using velocity commands, let's build an inner loop controller that will take those velocity commands and generate attitude/thrust commands to control the Crazyflie.

You may be thinking "why would I want to command attitude, when commanding a velocity gives me a good enough position controller!" And you would be right in thinking that! If your goal is solely to command a position in space and you have a drone that can do accurate velocity commands, then you may find you don't need to go this far. But, what if you want to be able to not just go from point A to point B, but do so with specific orientations in space? For example, what if you wanted to go through a window and you knew you needed to be perfectly leveled going through the window, or more interestingly, at a specific angle? This starts to go into the realm of flying specific trajectories, which we will get to a little later, but provides a little insight as to why you might end up commanding at this level if you do have velocity commands at your disposal. (Remember there is always the case where your drone only supports attitude commands!)

While the entire inner loop will be tested at once, we will build it in two steps:

1- Compute attitude commands from the horizontal velocity command

2- Compute the thrust command (total thrust) from the vertical velocity command

Attitude

We have already generated a velocity command for our position error, so let's build on that and use the cascaded structure we saw in the controls projects to generate an attitude command from our velocity command:

def velocity_controller(self, vel_cmd, vel):

    pitch = -self._kp_vel * (vel_cmd[0] - vel[0])  # note the sign change!  Remember + pitch is up, meaning it will send out drone backwards!
    roll = self._kp_vel * (vel_cmd[1] - vel[0])

    # add some limits
    pitch_cmd = np.clip(pitch, -self._bank_max, self._bank_max)
    roll_cmd = np.clip(roll, -self._bank_max, self._bank_max)

    ...

Thrust

The second half of the velocity controller is to convert the vertical velocity to a thrust command. Instead of true thrust, we will need to send a normalized thrust command, that is, a value between 0 and 1 to represent the thrust level, with 0 being no thrust and 1 being the max amount of thrust possible. This does mean we need to know how much thrust, in [N], is the max possible thrust of the crazyflie. We've done that calculation for you and you can find it as a constant at the top of the inner_controller.py file.

So let's again use the same process as the controls project to compute the thrust:

def velocity_controller(self, vel_cmd, vel)
    ...

    accel_cmd = self._kp_hdot * (hdot_cmd - hdot)  # compute acceleration from vertical velocity error
    accel_cmd = np.clip(accel_cmd, -self._haccel_max, self._haccel_max)  # saturate as desired
    thrust_cmd_N = DRONE_M * (accel_cmd + GRAVITY_MAG) / (np.cos(pitch_cmd) * np.cos(roll_cmd))  # compute thrust in N positive up

    # need to normalize the thrust
    thrust_cmd = thrust_cmd_N / MAX_THRUST_N

    return pitch_cmd, roll_cmd, thrust_cmd

Flying it!

Once you have decided on a gain, you can run this inner loop controller using the attitude_flyer.py script as follows:

python attitude_flyer.py --uri radio://0/80/2M

Where the uri passed in should be the uri you configured for your Crazyflie.